//------------------------------------------------------------------------------
// Simulated Lynx L6 Arm Service
//
// Professional Microsoft Robotics Developer Studio
//
// Updated for RDS 2008 (V2.0)
//
//------------------------------------------------------------------------------
using Microsoft.Ccr.Core;
using Microsoft.Ccr.Adapters.WinForms;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;

using System;
using System.Collections.Generic;

using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
using System.ComponentModel;

using submgr = Microsoft.Dss.Services.SubscriptionManager;
using armproxy = Microsoft.Robotics.Services.ArticulatedArm.Proxy;
using pmproxy = Microsoft.Robotics.PhysicalModel.Proxy;
using physicalmodel = Microsoft.Robotics.PhysicalModel;
using simwebcam = Microsoft.Robotics.Services.Simulation.Sensors.SimulatedWebcam.Proxy;
using W3C.Soap;

using xna = Microsoft.Xna.Framework;

namespace ProMRDS.Simulation.LynxL6Arm
{
    [DisplayName("SimulatedLynxL6Arm")]
    [Description("Simulation of a Lynx L6 Robotic Arm")]
    [Contract(Contract.Identifier)]
    [AlternateContract("http://schemas.microsoft.com/2006/06/articulatedarm.html")]

    public class SimulatedLynxL6ArmService : DsspServiceBase
    {
        #region Simulation Variables
        SimulationEnginePort _simEngine;
        SimulatedLynxL6Arm _l6Arm;
        SimulationEnginePort _notificationTarget;
        #endregion

        [Partner("Engine",
            Contract = engineproxy.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        private engineproxy.SimulationEnginePort _engineStub =
            new engineproxy.SimulationEnginePort();

        // Main service port
        [ServicePort("/SimulatedLynxL6Arm", AllowMultipleInstances = false)]
        private SimulatedLynxL6ArmOperations _mainPort =
            new SimulatedLynxL6ArmOperations();

        [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        private submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort();

        [AlternateServicePort("/arm", AlternateContract = armproxy.Contract.Identifier)]
        private armproxy.ArticulatedArmOperations _armPort = new armproxy.ArticulatedArmOperations();

        private Lynx6ArmState _state = new Lynx6ArmState();
        private Dictionary<string, pmproxy.Joint> _jointLookup;
        private Dictionary<string, int> _channelLookup;
        private Lynx6ArmConfiguration _armConfig;

        // This port receives events from the user interface
        FromWinformEvents _fromWinformPort = new FromWinformEvents();

        SimulatedLynxL6ArmUI _UI = null;

        const int DominoCount = 10;
        VisualEntity[] Dominos = new VisualEntity[DominoCount];

        bool _moveSequenceActive = false;

        public SimulatedLynxL6ArmService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        protected override void Start()
        {
            _simEngine = SimulationEngine.GlobalInstancePort;
            _notificationTarget = new SimulationEnginePort();

            // request a notification when the arm is inserted into the sim engine
            EntitySubscribeRequestType esrt = new EntitySubscribeRequestType();
            esrt.Name = "L6Arm";
            _simEngine.Subscribe(esrt, _notificationTarget);

            base.Start();

            // Add the winform message handler to the interleave
            MainPortInterleave.CombineWith(new Interleave(
                new TeardownReceiverGroup(),
                new ExclusiveReceiverGroup
                (
                    Arbiter.ReceiveWithIterator<armproxy.SetEndEffectorPose>(true, _armPort, SetEndEffectorHandler),
                    Arbiter.ReceiveWithIterator<armproxy.SetJointTargetPose>(true, _armPort, SetJointPoseHandler),
                    Arbiter.ReceiveWithIterator<armproxy.SetJointTargetVelocity>(true, _armPort, SetJointVelocityHandler),
                    Arbiter.Receive<InsertSimulationEntity>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime),
                    Arbiter.ReceiveWithIterator<FromWinformMsg>(true, _fromWinformPort, OnWinformMessageHandler)
                ),
                new ConcurrentReceiverGroup
                (
                    Arbiter.ReceiveWithIterator<armproxy.Subscribe>(true, _armPort, SubscribeHandler),
                    Arbiter.ReceiveWithIterator<armproxy.GetEndEffectorPose>(true, _armPort, GetEndEffectorHandler),
                    Arbiter.Receive<DsspDefaultLookup>(true, _armPort, DefaultLookupHandler),
                    Arbiter.ReceiveWithIterator<armproxy.Get>(true, _armPort, ArmGetHandler)
                )
            ));

            // Set the initial viewpoint
            SetupCamera();
            
            // Set up the world
            PopulateWorld();

            // Set up the state for generic contract
            InitializeArmState();

        }

        // Set up initial view
        private void SetupCamera()
        {
            CameraView view = new CameraView();
            view.EyePosition = new Vector3(0.05f, 0.42f, 0.47f);
            view.LookAtPoint = new Vector3(-0.28f, -1.38f, -2.34f);
            SimulationEngine.GlobalInstancePort.Update(view);
        }

        private void PopulateWorld()
        {
            AddSky();
            AddGround();
            AddBlocks();

            // Add an overhead camera
            AddCamera();

            // Create and place the dominos
            SpawnIterator(CreateDominos);

            // Create a LynxL6Arm Entity positioned at the origin
            SimulatedLynxL6Arm l6Arm = new SimulatedLynxL6Arm("L6Arm", new Vector3(0, 0, 0));
            SimulationEngine.GlobalInstancePort.Insert(l6Arm);
        }

        void AddSky()
        {
            // Add a sky using a static texture. We will use the sky texture
            // to do per pixel lighting on each simulation visual entity
            SkyDomeEntity sky = new SkyDomeEntity("skydome.dds", "sky_diff.dds");
            SimulationEngine.GlobalInstancePort.Insert(sky);

            // Add a directional light to simulate the sun.
            LightSourceEntity sun = new LightSourceEntity();
            sun.State.Name = "Sun";
            sun.Type = LightSourceEntityType.Directional;
            sun.Color = new Vector4(0.8f, 0.8f, 0.8f, 1);
            sun.Direction = new Vector3(0.5f, -.75f, 0.5f);
            SimulationEngine.GlobalInstancePort.Insert(sun);
        }

        void AddGround()
        {
            // create a large horizontal plane, at zero elevation.
            HeightFieldEntity ground = new HeightFieldEntity(
                "simple ground", // name
                "Wood_cherry.jpg", // texture image
                new MaterialProperties("ground",
                    0.2f, // restitution
                    0.5f, // dynamic friction
                    0.5f) // static friction
                );
            SimulationEngine.GlobalInstancePort.Insert(ground);
        }

        // Add a camera looking down on the arm
        void AddCamera()
        {
            // Low resolution, 60 degree Field of View
            CameraEntity cam = new CameraEntity(320, 240, ((float)Math.PI / 3));
            //CameraEntity cam = new AttachedCameraEntity();
            //cam.ViewSizeX = 320;
            //cam.ViewSizeY = 240;
            //cam.ViewAngle = ((float)Math.PI * 0.4f);
            cam.State.Name = "overheadwebcam";
            // Above the arm looking down
            Quaternion p;
            Quaternion q;
            p = Quaternion.FromAxisAngle(0, 0, 1, Conversions.DegreesToRadians(80));
            q = Quaternion.FromAxisAngle(0, 1, 0, (float)(Math.PI / 2));
            p = Quaternion.Multiply(p, q);
            //p = Quaternion.Normalize(p);

            //cam.State.Pose.Orientation = q;
            //cam.State.Pose.Position = new Vector3(0.0f, 0.75f, 0.0f);
            cam.State.Pose = new Pose(new Vector3(-0.10f, 0.6f, 0.0f), p);

            // camera renders in an offline buffer at each frame
            // required for service
            cam.IsRealTimeCamera = true;

            // Start simulated webcam service
            simwebcam.Contract.CreateService(
                ConstructorPort,
                Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
                    "http://localhost/" + cam.State.Name)
            );

            SimulationEngine.GlobalInstancePort.Insert(cam);
        }


        // Positions are in meters
        // NOTE: The Y coord should be half of the height
        Vector3[] _blockPositions =
            {
                new Vector3(-0.30f, 0.04f, 0.0f),
                new Vector3(-0.30f, 0.04f, 0.10f)
            };
        // Dimensions are in meters
        Vector3[] _blockDimensions =
            {
                new Vector3(0.03f, 0.08f, 0.03f),
                new Vector3(0.03f, 0.08f, 0.03f)
            };
        // Masses are in kilograms
        // They should be relatively small
        float[] _blockMasses =
            {
                0.05f,
                0.08f
            };
        // Colors are R,G, B (range 0 to 255 for each)
        Vector3[] _blockColors =
            {
                new Vector3(255, 0, 0),
                new Vector3(0, 255, 0)
            };

        // keep track of the blocks
        VisualEntity[] _blocks = new VisualEntity[2];

        void AddBlocks()
        {
            BoxShapeProperties cBoxShape = null;
            for (int i = 0; i < _blockPositions.Length; i++)
            {
                // Create simple entity, with a single shape
                cBoxShape = new BoxShapeProperties(
                        _blockMasses[i],        // mass in kilograms
                        new Pose(),             // relative pose
                        _blockDimensions[i]);   // dimensions
                cBoxShape.Material = new MaterialProperties("gbox", 1.0f, 0.4f, 0.5f);

                cBoxShape.DiffuseColor.X = (float)(_blockColors[i].X / 255.0);
                cBoxShape.DiffuseColor.Y = (float)(_blockColors[i].Y / 255.0);
                cBoxShape.DiffuseColor.Z = (float)(_blockColors[i].Z / 255.0);
                cBoxShape.DiffuseColor.W = 0.5f;

                _blocks[i] = new SingleShapeEntity(new BoxShape(cBoxShape), _blockPositions[i]);

                // Name the entity. All entities must have unique names.
                _blocks[i].State.Name = "block_" + i.ToString();

                // Insert entity into simulation
                SimulationEngine.GlobalInstancePort.Insert(_blocks[i]);
            }
        }

        void ResetBlocks()
        {
            int i = 0;
            foreach (VisualEntity block in _blocks)
            {
                _blocks[i].DeferredTaskQueue.Post(new Task<int>(i, DeferredSetBlockPose));
                i++;
            }
        }

        // This method sets the pose of a block in its update method
        void DeferredSetBlockPose(int i)
        {
            _blocks[i].PhysicsEntity.SetPose(new Pose(_blockPositions[i]));
        }

        // Returns the initial position of the requested domino
        Vector3 InitialDominoPosition(int i)
        {
            return new Vector3(Conversions.InchesToMeters(2.5f) + i * 0.011f, 0, 0);
        }

        // Create and place DominoCount dominos
        public IEnumerator<ITask> CreateDominos()
        {
            for(int i=0; i<DominoCount; i++)
            {
                Dominos[i] = new Domino(InitialDominoPosition(i), i);
                SimulationEngine.GlobalInstancePort.Insert(Dominos[i]);
            }

            yield break;
        }

        // Place the dominos back in their initial positions
        public void ResetDominos()
        {
            for (int i = 0; i < DominoCount; i++)
            {
                Dominos[i].State.Pose = new Pose(InitialDominoPosition(i));
                // Don't move the domino until its Update method is executing
                Dominos[i].DeferredTaskQueue.Post(new Task<int>(i, DeferredSetDominoPose));
            }
        }

        // This method sets the pose of a domino in its update method
        void DeferredSetDominoPose(int i)
        {
            Dominos[i].PhysicsEntity.SetPose(Dominos[i].State.Pose);
        }

        // Restore the arm to its initial position
        public void ResetArmPosition()
        {
            _l6Arm.State.Pose = new Pose(new Vector3(0, 0, 0));
            // Don't set the pose until the Update method executes
            _l6Arm.DeferredTaskQueue.Post(new Task(DeferredSetArmPose));
            // wait for the arm motion to settle before making it non-kinematic
            Activate(Arbiter.Receive(false, TimeoutPort(500), delegate(DateTime now) 
            {
                _l6Arm.DeferredTaskQueue.Post(new Task(ReleaseArm));
            }));
        }

        // Sets the arm pose in its update method
        void DeferredSetArmPose()
        {
            _l6Arm.PhysicsEntity.IsKinematic = true;
            _l6Arm.PhysicsEntity.SetPose(_l6Arm.State.Pose);
        }

        // Restores the arm to non-kinematic
        void ReleaseArm()
        {
            _l6Arm.PhysicsEntity.IsKinematic = false;
        }

        //------------------- Arm Movement Methods ------------------------

        // This method is executed if the MoveTo method fails
        void ShowError(Exception e)
        {
            Console.WriteLine(e.Message);
        }

        // This method puts the arm into its parked position
        public IEnumerator<ITask> ParkArm()
        {
            // Raise the grippers away from the working surface
            yield return Arbiter.Choice(_l6Arm.MoveTo(0, 0, 80, 0, 0, 0.05f, 2),
                delegate(SuccessResult s) { },
                ShowError);

            // Move to the parked position
            yield return Arbiter.Choice(_l6Arm.MoveTo(0, 80, -56, -75.2f, 0, 0.05f, 3),
                delegate(SuccessResult s) { },
                ShowError);
        }

        // utility method to check for early termination of the motion and to call MoveToPosition
        ITask CheckAndMove(float x, float y, float z, float gripAngle, float gripRotation, float grip, float time)
        {
            if (_moveSequenceActive)
            {
                return Arbiter.Choice(MoveToPosition(x, y, z, gripAngle, gripRotation, grip, time),
                    delegate(SuccessResult s) { },
                    ShowError);
            }
            else
            {
                return new Task(DoNothing);
            }
        }

        void DoNothing()
        {
        }

        // This method executes a series of arm movements that cause the arm to pick up 
        // dominos from one side and put them down in reverse order on the other side.
        public IEnumerator<ITask> ReverseDominos()
        {
            _moveSequenceActive = true;
            for (int i = 0; i < DominoCount; i++)
            {
                Vector3 src = InitialDominoPosition(DominoCount - i - 1);

                // move the arm into position to grasp the domino
                yield return CheckAndMove(0.18f, 0.05f, src.Z, -80, 0, 0.04f, 3);
                yield return CheckAndMove(src.X, 0.025f, src.Z, -80, 0, 0.04f, 1f);
                // close the grip to grab the domino
                yield return CheckAndMove(src.X, 0.025f, src.Z, -80, 0, 0.025f, 0.5f);
                // move it out and up
                yield return CheckAndMove(0.17f, 0.05f, src.Z, -80, 0, 0.025f, 0.5f);

                Vector3 dst = InitialDominoPosition(i);

                // move it to the other side
                yield return CheckAndMove(-0.17f, 0.05f, dst.Z, -80, 0, 0.025f, 3);
                // move it into position
                yield return CheckAndMove(-dst.X, 0.05f, dst.Z, -80, 0, 0.025f, 1);
                // lower it
                yield return CheckAndMove(-dst.X, 0.026f, dst.Z, -80, 0, 0.025f, 1);
                // and release it
                yield return CheckAndMove(-dst.X, 0.06f, dst.Z, -80, 0, 0.04f, 0.5f);
                // move the arm away from the destination dominos to prepare for the next one
                yield return CheckAndMove(-0.17f, 0.05f, dst.Z, -80, 0, 0.04f, 1);
            }
            yield break;
        }

        // The destination positions for the dominos in the topple movement
        Vector3[] ToppleDominoPosition = new Vector3[]
        {
            new Vector3(-0.11f,0,0.125f),
            new Vector3(-0.12f,0,0.10f),
            new Vector3(-0.125f,0,0.075f),
            new Vector3(-0.12f,0,0.05f),
            new Vector3(-0.11f,0,0.025f),
            new Vector3(-0.10f,0,0.00f),
            new Vector3(-0.095f,0,-0.025f),
            new Vector3(-0.10f,0,-0.05f),
            new Vector3(-0.11f,0,-0.075f),
            new Vector3(-0.12f,0,-0.10f)
        };

        // This method executes a series of arm movements that cause the arm to pick up 
        // dominos from one side and put them down in a pattern on the other side and 
        // then the arm causes the dominos to fall.
        public IEnumerator<ITask> ToppleDominos()
        {
            _moveSequenceActive = true;
            for (int i = 0; i < DominoCount; i++)
            {
                Vector3 src = InitialDominoPosition(DominoCount - i - 1);

                // move the arm into position to grasp the domino
                yield return CheckAndMove(0.18f, 0.06f, src.Z, -80, 0, 0.04f, 3);
                yield return CheckAndMove(src.X, 0.025f, src.Z, -90, 0, 0.04f, 1f);
                // close the grip to grab the domino
                yield return CheckAndMove(src.X, 0.025f, src.Z, -90, 0, 0.025f, 0.5f);
                // move it out and up
                yield return CheckAndMove(0.17f, 0.2f, src.Z, 0, 0, 0.025f, 1);
                // move it to the other side

                Vector3 dst = ToppleDominoPosition[i];
                yield return CheckAndMove(dst.X - 0.02f, 0.2f, dst.Z, 0, 0, 0.025f, 2);
                // move it into position
                yield return CheckAndMove(dst.X - 0.02f, 0.03f, dst.Z, -90, AdjustWrist(0, dst.X, dst.Z), 0.025f, 1);
                yield return CheckAndMove(dst.X, 0.03f, dst.Z, -90, AdjustWrist(0, dst.X, dst.Z), 0.025f, 0.5f);
                // lower it
                yield return CheckAndMove(dst.X, 0.026f, dst.Z, -90, AdjustWrist(0, dst.X, dst.Z), 0.025f, 1);
                // release it
                yield return CheckAndMove(dst.X, 0.026f, dst.Z, -90, AdjustWrist(0, dst.X, dst.Z), 0.04f, 0.5f);
                // back away
                yield return CheckAndMove(dst.X, 0.06f, dst.Z, -80, AdjustWrist(0, dst.X, dst.Z), 0.04f, 1);
            }
            // knock them down
            Vector3 final = ToppleDominoPosition[9];    // the last domino position
            yield return CheckAndMove(final.X, 0.05f, final.Z - 0.04f, -80, 0, 0, 1);
            yield return CheckAndMove(final.X, 0.05f, final.Z - 0.04f, -80, 0, 0, 0.5f);
            // pause for dramatic effect
            yield return Arbiter.Receive(false, TimeoutPort(500), delegate(DateTime now) { });
            yield return CheckAndMove(final.X, 0.05f, final.Z + 0.06f, -80, 0, 0, 0.3f);
            yield break;
        }

        // This method calculates the wrist rotation angle that will orient the wrist to 
        // the specified angle relative to the X axis.
        float AdjustWrist(float angle, float x, float z)
        {
            float val = angle + Conversions.RadiansToDegrees((float)(Math.Atan2(-z, -x))) + 90;
            while (val < -90)
                val += 180;

            while (val > 90)
                val -= 180;

            return val;
        }

        // This method generates random positions for the arm until it finds one that is valid
        public IEnumerator<ITask> RandomMove()
        {
            Random rnd = new Random();

            bool valid = false;
            float x, y, z, gripAngle, gripRotation;
            float r;

            while (!valid)
            {
                // Generate likely poses and create variables
                // to assist in debugging.
                x = (float)rnd.NextDouble() * (-0.2f) - 0.1f;
                y = (float)rnd.NextDouble() * 0.3f + 0.02f;
                z = (float)rnd.NextDouble() * 0.6f - 0.3f;
                gripAngle = (float)rnd.NextDouble() * 120f - 60f;
                gripRotation = (float)rnd.NextDouble() * 160f - 80f;
                
                // If the position seems plausible, try it
                // The radial range of the arm is about 11cm to 38cm,
                // but leave a little leeway
                r = (float)Math.Sqrt((x * x) + (z * z));
                if ((r > 0.12f) && (r < 0.37f))
                {
                yield return Arbiter.Choice(MoveToPosition(
                        x, y, z, gripAngle, gripRotation,
                    (float)rnd.NextDouble() * 0.05f,       // gripper spacing
                    1),   // time
                    delegate(SuccessResult s) { valid = true; },
                    delegate(Exception e)
                    {
                        // Get the message just for debugging
                        string msg = e.Message;
                    });
                }
            }
        }

        // Physical attributes of the arm

        float L1 = Conversions.InchesToMeters(4.75f);   // Length of upper arm
        float L2 = Conversions.InchesToMeters(4.75f);   // Length of lower arm

        // Not required
        //float Grip = Conversions.InchesToMeters(3.5f);  // Length of gripper

        // NOTE: The gripper is basically in two parts because it contains
        // the wrist rotate joint which is NOT at the same place as the
        // wrist joint.
        // Measurements are:
        // Wrist joint to rotate plate = 2.25"
        // Rotate place to tip of fingers = 3.5"
        // The actual length of the "hand" varies slightly depending on
        // whether the gripper is open or closed.
        // L3 measures the distance to the tip of the fingers or End Of Arm (EOA).
        // A Tool Center Point (TCP) would be shorter than this.
        // The rubber fingers are just over 1" long, so to keep the grip action
        // happening on the fingers, then L3 needs to be shorter by about 0.5".
        float L3 = Conversions.InchesToMeters(5.75f);   // Length of whole hand

        float H = Conversions.InchesToMeters(3f);     // Height of the base
        float G = Conversions.InchesToMeters(1.75f);    // Radius of the base

        // Perform the Inverse Kinematics calculations
        //
        bool InverseKinematics(
            float mx, // x position
            float my, // y position
            float mz, // z position
            float p,  // angle of the grip
            ref float baseAngle,
            ref float shoulder,
            ref float elbow,
            ref float wrist)
        {
            // Algorithm taken from Hoon Hong's ik2.xls IK method posted on the Lynx website

            float r = (float)Math.Sqrt(mx * mx + mz * mz); // horizontal distance to target
            baseAngle = (float)Math.Atan2(-mz, -mx); // angle to the target

            float pRad = Conversions.DegreesToRadians(p);
            float rb = (float)((r - L3 * Math.Cos(pRad)) / (2 * L1));
            float yb = (float)((my - H - L3 * Math.Sin(pRad)) / (2 * L1));
            float q = (float)(Math.Sqrt(1 / (rb * rb + yb * yb) - 1));
            float p1 = (float)(Math.Atan2(yb + q * rb, rb - q * yb));
            float p2 = (float)(Math.Atan2(yb - q * rb, rb + q * yb));
            shoulder = p1 - Conversions.DegreesToRadians(90);  // angle of the shoulder joint
            elbow = p2 - shoulder;                 // angle of the elbow joint
            wrist = pRad - p2;                     // angle of the wrist joint

            // Convert all values to degrees
            baseAngle = Conversions.RadiansToDegrees(baseAngle);
            shoulder = Conversions.RadiansToDegrees(shoulder);
            elbow = Conversions.RadiansToDegrees(elbow);
            wrist = Conversions.RadiansToDegrees(wrist);
            // Check to make sure that the solution is valid
            if (Single.IsNaN(baseAngle) ||
                Single.IsNaN(shoulder) ||
                Single.IsNaN(elbow) ||
                Single.IsNaN(wrist))
            {
                // Use for debugging only!
                //Console.WriteLine("No solution to Inverse Kinematics");
                return false;
            }
            else
                return true;
        }

        void ForwardKinematics(
            float baseAngle,
            float shoulder,
            float elbow,
            float wrist,
            ref float mx, // x position
            ref float my, // y position
            ref float mz, // z position
            ref float p)  // angle of the grip
        {
            float shoulderAngle, elbowAngle, wristAngle;
            float height, depth;

            // Accumulate the angles along the links
            shoulderAngle = Conversions.DegreesToRadians(90 - shoulder);
            elbowAngle = Conversions.DegreesToRadians(90 - elbow) + shoulderAngle;
            wristAngle = Conversions.DegreesToRadians(-wrist) + elbowAngle;
            // Sum up the height and depth for all links
            height = H;
            depth = 0;
            // Upper Arm L1
            height += (float) (L1 * Math.Sin(shoulderAngle));
            depth += (float) (L1 * Math.Cos(shoulderAngle));
            // Lower Arm L2
            height += (float) (L2 * Math.Sin(elbowAngle));
            depth += (float) (L2 * Math.Cos(elbowAngle));
            // Hand L3
            height += (float) (L3 * Math.Sin(wristAngle));
            depth += (float) (L3 * Math.Cos(wristAngle));

            // Calculate the 3D coordinates
            mx = (float)(-depth * Math.Sin(Conversions.DegreesToRadians(baseAngle - 90)));
            my = height;
            mz = (float)(depth * Math.Cos(Conversions.DegreesToRadians(baseAngle - 90)));
            // Now calculate the gripper angle relative to horizontal
            p = Conversions.RadiansToDegrees(wristAngle);
            // Flip the orientation because X is always negative
            p = 180 - p;
            // Force to the range +/-180
            if (p > 180)
                p = p - 360;
            if (p < -180)
                p = p + 360;
        }
        
        // This method calculates the joint angles necessary to place the arm into the 
        // specified position.  The arm position is specified by the X,Y,Z coordinates
        // of the gripper tip as well as the angle of the grip, the rotation of the grip, 
        // and the open distance of the grip.  The motion is completed in the 
        // specified time.
        public SuccessFailurePort MoveToPosition(
            float mx, // x position
            float my, // y position
            float mz, // z position
            float p, // angle of the grip
            float w, // rotation of the grip
            float grip, // distance the grip is open
            float time) // time to complete the movement
        {
            float baseAngle = 0, shoulder = 0, elbow = 0, wrist = 0;

            // Do the inverse kinematics to obtain the joint angles
            if (!InverseKinematics(mx, my, mz, p, ref baseAngle, ref shoulder, ref elbow, ref wrist))
            {
                SuccessFailurePort s = new SuccessFailurePort();
                s.Post(new Exception("Inverse Kinematics failed"));
                return s;
            }

            // Update the form with these parameters
            WinFormsServicePort.FormInvoke(
                delegate()
                {
                    _UI.SetPositionText(mx, my, mz, p, w, grip, time);
                });

            // Position the arm with the calculated joint angles.
            return _l6Arm.MoveTo(
                baseAngle,
                shoulder,
                elbow,
                wrist,
                w,
                grip,
                time);
        }


        #region Generic Arm Contract

        private void InitializeArmState()
        {
            if (_state == null)
            {
                _state = new Lynx6ArmState();
            }
            _armConfig = new Lynx6ArmConfiguration();
            _state.Joints = _armConfig.Joints;

            _jointLookup = new Dictionary<string, pmproxy.Joint>();
            _channelLookup = new Dictionary<string, int>();

            for (int i = 0; i < _state.Joints.Count; i++)
            {
                pmproxy.Joint j = _state.Joints[i];
                j.State.Angular.DriveTargetOrientation = new pmproxy.Quaternion(0, 0, 0, 1);

                // Joint speeds are confusing. Pulse widths range from 500uS to 2500uS for
                // a range of -90 to 90 degrees. So 1000uS is equivalent to 90 degrees,
                // so a speed of 100uS per second will take 10 seconds to travel 90 degrees.
                //
                // Initialize joint speeds to be 500uS per second (takes 2 seconds to move 90 degrees)
                pmproxy.Vector3 angularVelocity = ProxyConversion.ToProxy(physicalmodel.Vector3.Scale(
                                            ProxyConversion.FromProxy(j.State.Connectors[0].JointAxis), 500f));
                j.State.Angular.DriveTargetVelocity = angularVelocity;

                // create dictionary for quick lookup of joints from name
                _jointLookup.Add(j.State.Name, j);
                _channelLookup.Add(j.State.Name, i);

            }

        }

        #region Generic Operations Handlers

        // NOTE:
        // Some of these operations are not yet implemented!

        /*
        /// <summary>
        /// HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(Microsoft.Dss.Core.DsspHttp.HttpGet get)
        {
            get.ResponsePort.Post(new dssphttp.HttpResponseType(_state));
            yield break;
        }
        */

        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        //[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReliableSubscribeHandler(armproxy.ReliableSubscribe subscribe)
        {
            SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }
        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        //[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> SubscribeHandler(armproxy.Subscribe subscribe)
        {
            SubscribeRequestType request = subscribe.Body;
            SubscribeHelper(_submgrPort, request, subscribe.ResponsePort);
            yield break;
        }

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        /// <remarks>Get handler for Generic Arm contract</remarks>
        //[ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> ArmGetHandler(armproxy.Get get)
        {
            // Update the state first with the current joint info
            float baseAngle = 0, shoulder = 0, elbow = 0, wrist = 0;
            float rotate = 0, gripper = 0;
            if (_l6Arm != null)
            {
                // Get the target joint angles for all joints
                _l6Arm.GetTargetJointAngles(out baseAngle, out shoulder, out elbow, out wrist, out rotate, out gripper);
                baseAngle = ServoAngleToJointAngle(0, (int)baseAngle);
                shoulder = ServoAngleToJointAngle(1, (int)shoulder);
                elbow = ServoAngleToJointAngle(2, (int)elbow);
                wrist = ServoAngleToJointAngle(3, (int)wrist);
                rotate = ServoAngleToJointAngle(4, (int)rotate);
                // The gripper has to be handled differently
                //gripper = ServoAngleToJointAngle(5, (int)gripper);
                gripper = (gripper / Conversions.InchesToMeters(2)) * 70 - 35;

                // Put the values into the state
                _state.Joints[0].State.Angular.DriveTargetOrientation = AngleToOrientationQuaternion(baseAngle);
                _state.Joints[1].State.Angular.DriveTargetOrientation = AngleToOrientationQuaternion(shoulder);
                _state.Joints[2].State.Angular.DriveTargetOrientation = AngleToOrientationQuaternion(elbow);
                _state.Joints[3].State.Angular.DriveTargetOrientation = AngleToOrientationQuaternion(wrist);
                _state.Joints[4].State.Angular.DriveTargetOrientation = AngleToOrientationQuaternion(rotate);
                _state.Joints[5].State.Angular.DriveTargetOrientation = AngleToOrientationQuaternion(gripper);
            }
            // Return the state
            get.ResponsePort.Post(_state);
            yield break;
        }


        //Convert an angle in degrees to an orientation quaternion
        private pmproxy.Quaternion AngleToOrientationQuaternion(float angle)
        {
            physicalmodel.AxisAngle rotate = new physicalmodel.AxisAngle(new physicalmodel.Vector3(1, 0, 0),
                                                (float)(Math.PI * (double)angle / 180));
            // joint axis is always the positive X axis
            pmproxy.Quaternion q = ProxyConversion.ToProxy(physicalmodel.Quaternion.FromAxisAngle(rotate));
            return q;
        }

        /// <summary>
        /// Set End Effector
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        /// <remarks>Moves the arm end effector to a specified pose</remarks>
        private IEnumerator<ITask> SetEndEffectorHandler(armproxy.SetEndEffectorPose update)
        {
            bool success = false;
            float x, y, z, p;
            float baseAngle = 0, shoulder = 0, elbow = 0, wrist = 0;
            float baseAngle0 = 0, shoulder0 = 0, elbow0 = 0, wrist0 = 0;
            float rotateVal, gripperVal;
            AxisAngle a;

            // Get the target joint angles for all joints
            // We have to do this so that we can preserve the values for the joints
            // that are NOT being changed. Note that we do not use the CURRENT joint
            // angles because this could potentially stop a motion in progress.
            _l6Arm.GetTargetJointAngles(out baseAngle, out shoulder, out elbow, out wrist, out rotateVal, out gripperVal);

            // Get all the pose information required
            // NOTE: The angle is assumed to be about the X axis of the gripper
            Quaternion q = new Quaternion(
                update.Body.EndEffectorPose.Orientation.X,
                update.Body.EndEffectorPose.Orientation.Y,
                update.Body.EndEffectorPose.Orientation.Z,
                update.Body.EndEffectorPose.Orientation.W);
            a = Quaternion.ToAxisAngle(q);
            p = Conversions.RadiansToDegrees(a.Angle*Math.Sign(a.Axis.X));
            x = update.Body.EndEffectorPose.Position.X;
            y = update.Body.EndEffectorPose.Position.Y;
            z = update.Body.EndEffectorPose.Position.Z;

            baseAngle0 = baseAngle;
            shoulder0 = shoulder;
            elbow0 = elbow;
            wrist0 = wrist;

            // Use Inverse Kinematics to calculate the joint angles
            if (!InverseKinematics(x, y, z, p, ref baseAngle, ref shoulder, ref elbow, ref wrist))
            {
                // The results were not a valid pose!
                // Don't attempt the move or it might break the arm
                Fault f;
                // Should put something in the Fault!
                f = Fault.FromException(new Exception("No solution to Inverse Kinematics"));
                update.ResponsePort.Post(f);
                yield break;
            }

            // calculate the time needed to make the motion
            float maxAngle = Math.Max(Math.Abs(baseAngle0 - baseAngle), Math.Abs(shoulder0 - shoulder));
            maxAngle = Math.Max(maxAngle, Math.Abs(elbow0 - elbow));
            maxAngle = Math.Max(maxAngle, Math.Abs(wrist0 - wrist));
            float time = maxAngle * 11f / 360f;
            
            // Set the arm
            if (time <= 0.1f)
            {
                time = 0.1f;
            }

            yield return Arbiter.Choice(_l6Arm.MoveTo(baseAngle, shoulder, elbow,
                            wrist, rotateVal, gripperVal, time),
                delegate(SuccessResult s) { success = true; },
                ShowError);

            if (success)
                update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            else
            {
                Fault f;
                // Should put something in the Fault!
                f = Fault.FromException(new Exception("MoveTo failed"));
                update.ResponsePort.Post(f);
            }

            // NOT FINISHED!
            // we send a replace notification since end effector pose updates affect all joints
            armproxy.Replace replace = new armproxy.Replace();
            replace.Body = (armproxy.ArticulatedArmState)_state;
            base.SendNotification(_submgrPort, replace);

            // send an end effector update notification as well
            base.SendNotification(_submgrPort, update);

            yield break;

        }


        /// <summary>
        /// Get End Effector
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        /// <remarks>Gets the current pose of the arm end effector</remarks>
        private IEnumerator<ITask> GetEndEffectorHandler(armproxy.GetEndEffectorPose get)
        {
            float x = 0, y = 0, z = 0, p = 0;
            float baseAngle = 0, shoulder = 0, elbow = 0, wrist = 0;
            float rotateVal, gripperVal;

            // Get the current joint angles for all joints
            _l6Arm.GetCurrentJointAngles(out baseAngle, out shoulder, out elbow, out wrist, out rotateVal, out gripperVal);

            ForwardKinematics(baseAngle, shoulder, elbow, wrist, ref x, ref y, ref z, ref p);

            // Create a response
            armproxy.GetEndEffectorPoseResponse response = new armproxy.GetEndEffectorPoseResponse();
            // What a pain in the neck!!!
            // All because standard classes cannot be assigned to their proxies!
            pmproxy.Pose rp = new pmproxy.Pose();
            pmproxy.Vector3 v = new pmproxy.Vector3(x, y, z);
            rp.Position = v;
            Quaternion q = new Quaternion();
            q = Quaternion.FromAxisAngle(1, 0, 0, Conversions.DegreesToRadians(p));
            pmproxy.Quaternion qp = new pmproxy.Quaternion(q.X, q.Y, q.Z, q.W);
            rp.Orientation = qp;
            response.EndEffectorPose = rp;

            // Send back the response
            get.ResponsePort.Post(response);

            yield break;

        }

        // Default movement time
        float defaultTime = 0.01f;

        /// <summary>
        /// Set Joint Pose
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        /// <remarks>Sets the pose of a single joint (in most cases this is just an angle)</remarks>
        private IEnumerator<ITask> SetJointPoseHandler(armproxy.SetJointTargetPose update)
        {
            float baseVal, shoulderVal, elbowVal, wristVal, rotateVal, gripperVal;
            int index;
            bool success = false;
            string name = update.Body.JointName;
            pmproxy.Joint j = _jointLookup[name];
            pmproxy.AxisAngle orientation = update.Body.TargetOrientation;
            float orientationAngle = orientation.Angle * Math.Sign(orientation.Axis.X);
            float moveTime, angleDiff;

            // Find out which joint it is from the name
            index = _channelLookup[name];
            // Convert the angle
            float servoAngle = (float) JointAngleToServoAngle(index, (int)(orientationAngle * 180 / Math.PI));
            // Get the target joint angles for all joints
            // We have to do this so that we can preserve the values for the joints
            // that are NOT being changed. Note that we do not use the CURRENT joint
            // angles because this could potentially stop a motion in progress.
            _l6Arm.GetTargetJointAngles(out baseVal, out shoulderVal, out elbowVal, out wristVal, out rotateVal, out gripperVal);
            // Set the angle for the appropriate joint
            // Calculate the difference between the current angle and the
            // requested angle as well, and then figure out the time to take
            switch (index)
            {
                    // index values should be in an enum
                case 0:
                    angleDiff = Math.Abs(servoAngle - baseVal);
                    baseVal = servoAngle;
                    break;
                case 1:
                    angleDiff = Math.Abs(servoAngle - shoulderVal);
                    shoulderVal = servoAngle;
                    break;
                case 2:
                    angleDiff = Math.Abs(servoAngle - elbowVal);
                    elbowVal = servoAngle;
                    break;
                case 3:
                    angleDiff = Math.Abs(servoAngle - wristVal);
                    wristVal = servoAngle;
                    break;
                case 4:
                    angleDiff = Math.Abs(servoAngle - rotateVal);
                    rotateVal = servoAngle;
                    break;
                case 5:
                    // Convert the gripper value from +/-90 to 0-2 inches
                    // This is a fudge anyway
                    angleDiff = 10.0f;
                    gripperVal = ((servoAngle + 35) / 70) * Conversions.InchesToMeters(2);
                    break;
                default:
                    // Invalid index - Should never happen!
                    throw(new Exception("Invalid servo index"));
            }

            // Calculate the elapsed time for the move
            moveTime = angleDiff / _state.MaxAngularSpeed;
            // If the time is very small, ignore it
            if (moveTime < defaultTime)
                moveTime = defaultTime;

            // Process the new angles
            yield return Arbiter.Choice(_l6Arm.MoveTo(baseVal, shoulderVal, elbowVal,
                            wristVal, rotateVal, gripperVal, moveTime),
                delegate(SuccessResult s) { success = true; },
                ShowError);

            if (success)
                update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            else
            {
                Fault f = new Fault();
                // Should put something in the Fault!
                update.ResponsePort.Post(f);
            }

        }

        /// <summary>
        /// Set Joint Velocity
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        private IEnumerator<ITask> SetJointVelocityHandler(armproxy.SetJointTargetVelocity update)
        {
            pmproxy.Joint j = _jointLookup[update.Body.JointName];
            j.State.Angular.DriveTargetVelocity = update.Body.TargetVelocity;
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        #endregion

        #region Utility Functions

        //Convert from the servo angle reading to a LH joint angle
        private int ServoAngleToJointAngle(int index, int servoAngle)
        {
            // The index values should be an enum -- they are for the real arm
            switch (index)
            {
                case 0: // Base
                    return servoAngle;
                case 1: // Shoulder
                    return servoAngle;
                case 2: // Elbow
                    return servoAngle;
                case 3: // Wrist
                    return servoAngle;
                case 4: // Wrist Rotate
                    return servoAngle;
                case 5: // Gripper
                    return servoAngle;
            }
            return -1;
        }

        //Convert from the LH joint angle to a servo reading
        private int JointAngleToServoAngle(int index, int jointAngle)
        {
            switch (index)
            {
                case 0: // Base
                    return jointAngle;
                case 1: // Shoulder
                    return jointAngle;
                case 2: // Elbow
                    return jointAngle;
                case 3: // Wrist
                    return jointAngle;
                case 4: // Wrist Rotate
                    return jointAngle;
                case 5: // Gripper
                    return jointAngle;
            }
            return -1;
        }

        #endregion

        #endregion

        #region UI form methods
        // Create the UI form
        System.Windows.Forms.Form CreateForm()
        {
            return new SimulatedLynxL6ArmUI(_fromWinformPort);
        }

        // process messages from the UI Form
        IEnumerator<ITask> OnWinformMessageHandler(FromWinformMsg msg)
        {
            switch (msg.Command)
            {
                case FromWinformMsg.MsgEnum.Loaded:
                    // the windows form is ready to go
                    _UI = (SimulatedLynxL6ArmUI)msg.Object;
                    break;

                case FromWinformMsg.MsgEnum.MoveToPosition:
                    {
                        // move the arm to the specified position
                        MoveToPositionParameters moveParams = (MoveToPositionParameters)msg.Object;
                        yield return Arbiter.Choice(
                            MoveToPosition(moveParams.X, moveParams.Y, moveParams.Z,
                            moveParams.GripAngle, moveParams.GripRotation, moveParams.Grip, moveParams.Time),
                            delegate(SuccessResult s) { },
                            delegate(Exception e)
                            {
                                WinFormsServicePort.FormInvoke(
                                    delegate()
                                    {
                                        _UI.SetErrorText(e.Message);
                                    });
                            }
                        );

                        break;
                    }

                case FromWinformMsg.MsgEnum.Reset:
                    _moveSequenceActive = false;    // terminate sequence
                    ResetDominos();
                    ResetArmPosition();
                    ResetBlocks();
                    break;

                case FromWinformMsg.MsgEnum.ReverseDominos:
                    SpawnIterator(ReverseDominos);
                    break;

                case FromWinformMsg.MsgEnum.Park:
                    SpawnIterator(ParkArm);
                    break;

                case FromWinformMsg.MsgEnum.ToppleDominos:
                    SpawnIterator(ToppleDominos);
                    break;

                case FromWinformMsg.MsgEnum.RandomMove:
                    SpawnIterator(RandomMove);
                    break;
            }
            yield break;
        }
        #endregion
        void InsertEntityNotificationHandlerFirstTime(InsertSimulationEntity ins)
        {
            InsertEntityNotificationHandler(ins);

            // Create the user interface form
            WinFormsServicePort.Post(new RunForm(CreateForm));

            // Listen on the main port for requests and call the appropriate handler.
            MainPortInterleave.CombineWith(
                new Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup(
                        Arbiter.Receive<InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
                        Arbiter.Receive<DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler)
                    ),
                    new ConcurrentReceiverGroup()
                )
            );
        }

        void InsertEntityNotificationHandler(InsertSimulationEntity ins)
        {
            _l6Arm = (SimulatedLynxL6Arm)ins.Body;
            _l6Arm.ServiceContract = Contract.Identifier;
        }

        void DeleteEntityNotificationHandler(DeleteSimulationEntity del)
        {
            _l6Arm = null;
        }
    }

    /// <summary>
    /// Defines a new entity type that overrides the ParentJoint with 
    /// custom joint properties.  It also handles serialization and
    /// deserialization properly.  The simulated arm is built from
    /// these entities.
    /// </summary>
    [DataContract]
    public class SingleShapeSegmentEntity : SingleShapeEntity
    {
        private Joint _customJoint;

        [DataMember]
        public Joint CustomJoint
        {
            get { return _customJoint; }
            set { _customJoint = value; }
        }

        /// <summary>
        /// Default constructor
        /// </summary>
        public SingleShapeSegmentEntity() { }

        /// <summary>
        /// Initialization constructor
        /// </summary>
        /// <param name="shape"></param>
        /// <param name="initialPos"></param>
        public SingleShapeSegmentEntity(Shape shape, Vector3 initialPos)
            : base(shape, initialPos)
        {
        }

        // Override the Initialize method to add code to replace the ParentJoint
        public override void Initialize(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            base.Initialize(device, physicsEngine);

            // update the parent joint to match our custom joint parameters
            if (_customJoint != null)
            {
                if(ParentJoint != null)
                    PhysicsEngine.DeleteJoint((PhysicsJoint)ParentJoint);

                // restore the entity pointers in _customJoint after deserialization if necessary
                if (_customJoint.State.Connectors[0].Entity == null)
                    _customJoint.State.Connectors[0].Entity = FindConnectedEntity(_customJoint.State.Connectors[0].EntityName, this);

                if (_customJoint.State.Connectors[1].Entity == null)
                    _customJoint.State.Connectors[1].Entity = FindConnectedEntity(_customJoint.State.Connectors[1].EntityName, this);

                ParentJoint = _customJoint;
                PhysicsEngine.InsertJoint((PhysicsJoint)ParentJoint);
            }
        }

        // Traverse to the top-level parent and search all of its descendents for the specified entity name
        VisualEntity FindConnectedEntity(string name, VisualEntity me)
        {
            // find the parent at the top of the hierarchy
            while (me.Parent != null)
                me = me.Parent;

            // now traverse the hierarchy looking for the name
            return FindConnectedEntityHelper(name, me);
        }

        VisualEntity FindConnectedEntityHelper(string name, VisualEntity me)
        {
            if (me.State.Name == name)
                return me;

            foreach (VisualEntity child in me.Children)
            {
                VisualEntity result = FindConnectedEntityHelper(name, child);
                if (result != null)
                    return result;
            }

            return null;
        }

        /// <summary>
        /// Override the base PreSerialize method so that we can properly serialize joints
        /// </summary>
        public override void PreSerialize()
        {
            base.PreSerialize();
            PrepareJointsForSerialization();
        }
    }

    //------------------------------ The SimulatedLynxL6Arm Entity -----------------------------
    // The arm consists of a base entity of type SimulatedLynxL6Arm with the following child entities
    // which are of type SingleShapeSegmentEntity in the following hierarchy:
    // SimulatedLynxL6Arm
    //   L0Entity - Represents the turntable, joint is _joints[0] which controls base rotation
    //     L1Entity - Represents the upper arm segment, _joints[1] controls shoulder movement
    //       L2Entity - Represents the lower arm segment, _joints[2] controls elbow movement
    //         L3Entity - Represents part of the wrist, _joints[3] controls wrist bend
    //           L4Entity - Represents part of the wrist, _joints[4] controls wrist rotation
    //             LeftGripEntity - Represents part of the gripper, _joints[5].Joint controls gripper position
    //             RightGripEntity - Represents part of the gripper, _joints[5].Joint2 controls gripper position
    [DataContract]
    public class SimulatedLynxL6Arm : SingleShapeEntity
    {
        // physical attributes of the arm
        static float L1 = Conversions.InchesToMeters(4.75f);
        static float L2 = Conversions.InchesToMeters(4.75f);
        static float Grip = Conversions.InchesToMeters(2.5f);
        static float L3 = Conversions.InchesToMeters(5.75f) - Grip;
        static float L4 = 0.03f;
        static float H = Conversions.InchesToMeters(3f);
        static float G = Conversions.InchesToMeters(2f);
        static float L1Radius = Conversions.InchesToMeters(0.7f);
        static float L2Radius = Conversions.InchesToMeters(0.7f);
        static float L3Radius = Conversions.InchesToMeters(0.7f);
        static float GripRadius = Conversions.InchesToMeters(0.2f);

        // This class holds a description of each of the joints in the arm.
        class JointDesc
        {
            public string Name;
            public float Min;  // minimum allowable angle
            public float Max;  // maximum allowable angle
            public PhysicsJoint Joint; // Phyics Joint
            public PhysicsJoint Joint2; // Alternate Physics Joint (used for gripper)
            public float Target;  // Target joint position
            public float Current;  // Current joint position
            public float Speed;  // Rate of moving toward the target position
            public JointDesc(string name, float min, float max)
            {
                Name = name; Min = min; Max = max;
                Joint = null;
                Joint2 = null;
                Current = Target = 0;
                Speed = 30;
            }
            
            // Returns true if the specified target is within the valid bounds
            public bool ValidTarget(float target)
            {
                return ((target >= Min) && (target <= Max));
            }
            
            // Returns true if the joint is not yet at the target position
            public bool NeedToMove(float epsilon)
            {
                if (Joint == null) return false;
                return (Math.Abs(Target - Current) > epsilon);
            }
            
            // Takes one step toward the target position based on the specified time
            public void UpdateCurrent(double time)
            {
                float delta = (float)(time * Speed);
                if (Target > Current)
                    Current = Math.Min(Current + delta, Target);
                else
                    Current = Math.Max(Current - delta, Target);
            }
        }

        // Initialize an array of descriptions for each joint in the arm
        JointDesc[] _joints = new JointDesc[]
        {
            new JointDesc("Base", -180, 180),
            new JointDesc("Shoulder", -90, 90),
            new JointDesc("Elbow", -65, 115),
            new JointDesc("Wrist", -90, 90),
            new JointDesc("WristRotate", -90, 90),
            new JointDesc("Gripper", 0, Conversions.InchesToMeters(2))
        };

        // default constructor (used in the case of deserialization)
        public SimulatedLynxL6Arm() { }

        // initialize constructor
        public SimulatedLynxL6Arm(string name, Vector3 position)
        {
            // The physics shape for the base is slightly lower than the actual base 
            // so that the upper-arm segment does not intersect it as it moves around.
            float baseHeight = H - L1Radius - 0.001f;
            State.Name = name;
            State.Pose.Position = position;
            State.Pose.Orientation = new Quaternion(0, 0, 0, 1);
            State.Assets.Mesh = "L6_Base.obj";
            MeshTranslation = new Vector3(0, 0.026f, 0);

            // build the base
            BoxShape = new BoxShape(new BoxShapeProperties(
                "Base",
                150, // mass
                new Pose(new Vector3(0, baseHeight / 2, 0), new Quaternion(0, 0, 0, 1)),
                new Vector3(G * 2, baseHeight, G * 2)));

            State.Flags |= EntitySimulationModifiers.Kinematic;

            // build and position L0 (top of the base)
            SphereShape L0Sphere = new SphereShape(new SphereShapeProperties(
                "L0Sphere",
                50,
                new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1)),
                L1Radius));

            SingleShapeSegmentEntity L0Entity = new SingleShapeSegmentEntity(L0Sphere, position + new Vector3(0, H, 0));
            L0Entity.State.Pose.Orientation = new Quaternion(0, 0, 0, 1);
            L0Entity.State.Name = name + "_L0";
            L0Entity.State.Assets.Mesh = "L6_L0.obj";
            L0Entity.MeshTranslation = new Vector3(0, -0.02f, 0);
            JointAngularProperties L0Angular = new JointAngularProperties();
            L0Angular.Swing1Mode = JointDOFMode.Free;
            L0Angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000);
            EntityJointConnector[] L0Connectors = new EntityJointConnector[2]
            {
                new EntityJointConnector(L0Entity, new Vector3(0,1,0), new Vector3(1,0,0), new Vector3(0, 0, 0)),
                new EntityJointConnector(this, new Vector3(0,1,0), new Vector3(1,0,0), new Vector3(0, H, 0))
            };
            L0Entity.CustomJoint = new Joint();
            L0Entity.CustomJoint.State = new JointProperties(L0Angular, L0Connectors);
            L0Entity.CustomJoint.State.Name = "BaseJoint";

            this.InsertEntityGlobal(L0Entity);

            // build and position L1 (upper arm)
            CapsuleShape L1Capsule = new CapsuleShape(new CapsuleShapeProperties(
                "L1Capsule",
                2,
                new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1)),
                L1Radius,
                L1));

            SingleShapeSegmentEntity L1Entity = new SingleShapeSegmentEntity(L1Capsule, position + new Vector3(0, H, 0));
            L1Entity.State.Pose.Orientation = new Quaternion(0, 0, 0, 1);
            L1Entity.State.Name = name + "_L1";
            L1Entity.State.Assets.Mesh = "L6_L1.obj";
            JointAngularProperties L1Angular = new JointAngularProperties();
            L1Angular.TwistMode = JointDOFMode.Free;
            L1Angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000);
            EntityJointConnector[] L1Connectors = new EntityJointConnector[2]
            {
                new EntityJointConnector(L1Entity, new Vector3(0,1,0), new Vector3(0,0,1), new Vector3(0, -L1/2, 0)),
                new EntityJointConnector(L0Entity, new Vector3(0,1,0), new Vector3(0,0,1), new Vector3(0, 0, 0))
            };
            L1Entity.CustomJoint = new Joint();
            L1Entity.CustomJoint.State = new JointProperties(L1Angular, L1Connectors);
            L1Entity.CustomJoint.State.Name = "Shoulder|-80|80|";

            L0Entity.InsertEntityGlobal(L1Entity);

            // build and position L2 (lower arm)
            CapsuleShape L2Capsule = new CapsuleShape(new CapsuleShapeProperties(
                "L2Capsule",
                2,
                new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1)),
                L2Radius,
                L2));

            SingleShapeSegmentEntity L2Entity = new SingleShapeSegmentEntity(L2Capsule, position + new Vector3(0, H, 0));
            // position the entity close to its final position once the joint is connected
            L2Entity.Position = new xna.Vector3(-0.07f, 0.2f, 0);
            L2Entity.Rotation = new xna.Vector3(177.07f, -174.04f, 89.73f);
            L2Entity.State.Name = name + "_L2";
            L2Entity.State.Assets.Mesh = "L6_L2.obj";
            JointAngularProperties L2Angular = new JointAngularProperties();
            L2Angular.TwistMode = JointDOFMode.Free;
            L2Angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000);
            EntityJointConnector[] L2Connectors = new EntityJointConnector[2]
            {
                new EntityJointConnector(L2Entity, new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, -L2/2, 0)),
                new EntityJointConnector(L1Entity, new Vector3(0,1,0), new Vector3(0,0,1), new Vector3(0, L1/2, 0))
            };
            L2Entity.CustomJoint = new Joint();
            L2Entity.CustomJoint.State = new JointProperties(L2Angular, L2Connectors);
            L2Entity.CustomJoint.State.Name = "Elbow|-80|80|";

            L1Entity.InsertEntityGlobal(L2Entity);

            // build and position L3 (first part of the wrist)
            CapsuleShape L3Capsule = new CapsuleShape(new CapsuleShapeProperties(
                "L3Capsule",
                2,
                new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1)),
                L3Radius,
                L3 - L4));

            SingleShapeSegmentEntity L3Entity = new SingleShapeSegmentEntity(L3Capsule, position + new Vector3(0, H, 0));
            // position the entity close to its final position once the joint is connected
            L3Entity.Position = new xna.Vector3(-0.17f, 0.2f, 0);
            L3Entity.Rotation = new xna.Vector3(177.4f, -174.36f, 89.66f);
            L3Entity.State.Name = name + "_L3";
            L3Entity.State.Assets.Mesh = "L6_L3.obj";
            L3Entity.MeshTranslation = new Vector3(0, 0.035f, 0);
            JointAngularProperties L3Angular = new JointAngularProperties();
            L3Angular.TwistMode = JointDOFMode.Free;
            L3Angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000);
            EntityJointConnector[] L3Connectors = new EntityJointConnector[2]
            {
                new EntityJointConnector(L3Entity, new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, -(L3-L4)/2, 0)),
                new EntityJointConnector(L2Entity, new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, L2/2, 0))
            };
            L3Entity.CustomJoint = new Joint();
            L3Entity.CustomJoint.State = new JointProperties(L3Angular, L3Connectors);
            L3Entity.CustomJoint.State.Name = "Wrist Bend|-80|80|";

            L2Entity.InsertEntityGlobal(L3Entity);

            // build and position L4 (second part of the wrist)
            CapsuleShape L4Capsule = new CapsuleShape(new CapsuleShapeProperties(
                "L4Capsule",
                2,
                new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1)),
                L3Radius,
                L4));

            L4Entity = new SingleShapeSegmentEntity(L4Capsule, position + new Vector3(0, H, 0));
            // position the entity close to its final position once the joint is connected
            L4Entity.Position = new xna.Vector3(-0.17f, 0.2f, 0);
            L4Entity.Rotation = new xna.Vector3(177.4f, -174.36f, 89.66f);
            L4Entity.State.Name = name + "_L4";
            L4Entity.State.Assets.Mesh = "L6_L4.obj";
            L4Entity.MeshTranslation = new Vector3(0, 0.021f, 0);
            JointAngularProperties L4Angular = new JointAngularProperties();
            L4Angular.Swing2Mode = JointDOFMode.Free;
            L4Angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000);
            EntityJointConnector[] L4Connectors = new EntityJointConnector[2]
            {
                new EntityJointConnector(L4Entity, new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, -L4/2, 0)),
                new EntityJointConnector(L3Entity, new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, (L3-L4)/2, 0))
            };
            L4Entity.CustomJoint = new Joint();
            L4Entity.CustomJoint.State = new JointProperties(L4Angular, L4Connectors);
            L4Entity.CustomJoint.State.Name = "Wrist Rotate";

            L3Entity.InsertEntityGlobal(L4Entity);

            // build and position LeftGrip
            CapsuleShape LeftGripCapsule = new CapsuleShape(new CapsuleShapeProperties(
                "LeftGripCapsule",
                1f,
                new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1)),
                GripRadius,
                Grip));
            LeftGripCapsule.State.DiffuseColor = new Vector4(0, 0, 0, 1);

            LeftGripEntity = new SingleShapeSegmentEntity(LeftGripCapsule, position + new Vector3(0, H, 0));
            // position the entity close to its final position once the joint is connected
            LeftGripEntity.Position = new xna.Vector3(-0.24f, 0.19f, 0.01f);
            LeftGripEntity.Rotation = new xna.Vector3(179.94f, -176.91f, 89.67f);
            LeftGripEntity.State.Name = name + "_LeftGrip";
            // use a linear joint for the left grip
            JointLinearProperties LeftGripLinear = new JointLinearProperties();
            LeftGripLinear.XMotionMode = JointDOFMode.Free;
            LeftGripLinear.XDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000);
            EntityJointConnector[] LeftGripConnectors = new EntityJointConnector[2]
            {
                new EntityJointConnector(LeftGripEntity, new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, -Grip/2, 0)),
                new EntityJointConnector(L4Entity, new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, L4/2, GripRadius))
            };
            LeftGripEntity.CustomJoint = new Joint();
            LeftGripEntity.CustomJoint.State = new JointProperties(LeftGripLinear, LeftGripConnectors);
            LeftGripEntity.CustomJoint.State.Name = "LeftGripJoint|-0.0254|0|";

            L4Entity.InsertEntityGlobal(LeftGripEntity);

            // build and position RightGrip
            CapsuleShape RightGripCapsule = new CapsuleShape(new CapsuleShapeProperties(
                "RightGripCapsule",
                1f,
                new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1)),
                GripRadius,
                Grip));
            RightGripCapsule.State.DiffuseColor = new Vector4(0, 0, 0, 1);

            RightGripEntity = new SingleShapeSegmentEntity(RightGripCapsule, position + new Vector3(0, H, 0));
            // position the entity close to its final position once the joint is connected
            RightGripEntity.Position = new xna.Vector3(-0.24f, 0.19f, 0);
            RightGripEntity.Rotation = new xna.Vector3(178.69f, -175.66f, 89.67f);
            RightGripEntity.State.Name = name + "_RightGrip";
            // use a linear joint for the right grip
            JointLinearProperties RightGripLinear = new JointLinearProperties();
            RightGripLinear.XMotionMode = JointDOFMode.Free;
            RightGripLinear.XDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000);
            EntityJointConnector[] RightGripConnectors = new EntityJointConnector[2]
            {
                new EntityJointConnector(RightGripEntity, new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, -Grip/2, 0)),
                new EntityJointConnector(L4Entity, new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, L4/2, -GripRadius))
            };
            RightGripEntity.CustomJoint = new Joint();
            RightGripEntity.CustomJoint.State = new JointProperties(RightGripLinear, RightGripConnectors);
            RightGripEntity.CustomJoint.State.Name = "RightGripJoint|0|0.0254|";

            L4Entity.InsertEntityGlobal(RightGripEntity);

            // Add a camera to see what the gripper is gripping
            AttachedCameraEntity gripCam = new AttachedCameraEntity();
            gripCam.State.Name = "Arm Cam";

            // move the camera above the L4entity and look at the grippers
            gripCam.State.Pose = new Pose(new Vector3(0.05f, -0.01f, 0),
                Quaternion.FromAxisAngle(0, 1, 0, (float)(Math.PI / 2)) *
                Quaternion.FromAxisAngle(1, 0, 0, (float)(Math.PI / 3)));

            // adjust the near plane so that we can see the grippers
            gripCam.Near = 0.01f;

            // the gripcam coordinates are relative to the L4Entity, don't use InsertEntityGlobal
            L4Entity.InsertEntity(gripCam);
        }

        // Get the Target joint angles but put them into separate variables
        public void GetTargetJointAngles(
                    out float baseVal,
                    out float shoulderVal,
                    out float elbowVal,
                    out float wristVal,
                    out float rotateVal,
                    out float gripperVal)
        {
            // Get the target values on the joint descriptors
            // NOTE: This assumes that the arm is already there!
            baseVal = _joints[0].Target;
            shoulderVal = _joints[1].Target;
            elbowVal = _joints[2].Target;
            wristVal = _joints[3].Target;
            rotateVal = _joints[4].Target;
            gripperVal = _joints[5].Target;
        }

        // Get the Current joint angles but put them into separate variables
        public void GetCurrentJointAngles(
                    out float baseVal,
                    out float shoulderVal,
                    out float elbowVal,
                    out float wristVal,
                    out float rotateVal,
                    out float gripperVal)
        {
            // Get the target values on the joint descriptors
            // NOTE: This assumes that the arm is already there!
            baseVal = _joints[0].Current;
            shoulderVal = _joints[1].Current;
            elbowVal = _joints[2].Current;
            wristVal = _joints[3].Current;
            rotateVal = _joints[4].Current;
            gripperVal = _joints[5].Current;
        }

        // These variables are used to keep track of the state of the arm while it is moving
        bool _moveToActive = false;
        const float _epsilon = 0.01f;
        SuccessFailurePort _moveToResponsePort = null;

        // This is the basic method used to move the arm.  The target position for each joint is specified along
        // with a time for the movement to be completed.  A port is returned which will receive a success message when the 
        // movement is completed or an exception message if an error is encountered.
        public SuccessFailurePort MoveTo(
            float baseVal, 
            float shoulderVal, 
            float elbowVal, 
            float wristVal, 
            float rotateVal, 
            float gripperVal, 
            float time)
        {
            SuccessFailurePort responsePort = new SuccessFailurePort();

            if (_moveToActive)
            {
                responsePort.Post(new Exception("Previous MoveTo still active."));
                return responsePort;
            }

            // check bounds.  If the target is invalid, post an exception message to the response port with a helpful error.
            if(!_joints[0].ValidTarget(baseVal))
            {
                responsePort.Post(new Exception(_joints[0].Name + "Joint set to invalid value: " + baseVal.ToString()));
                return responsePort;
            }

            if (!_joints[1].ValidTarget(shoulderVal))
            {
                responsePort.Post(new Exception(_joints[1].Name + "Joint set to invalid value: " + shoulderVal.ToString()));
                return responsePort;
            }

            if (!_joints[2].ValidTarget(elbowVal))
            {
                responsePort.Post(new Exception(_joints[2].Name + "Joint set to invalid value: " + elbowVal.ToString()));
                return responsePort;
            }

            if (!_joints[3].ValidTarget(wristVal))
            {
                responsePort.Post(new Exception(_joints[3].Name + "Joint set to invalid value: " + wristVal.ToString()));
                return responsePort;
            }

            if (!_joints[4].ValidTarget(rotateVal))
            {
                responsePort.Post(new Exception(_joints[4].Name + "Joint set to invalid value: " + rotateVal.ToString()));
                return responsePort;
            }

            if (!_joints[5].ValidTarget(gripperVal))
            {
                responsePort.Post(new Exception(_joints[5].Name + "Joint set to invalid value: " + gripperVal.ToString()));
                return responsePort;
            }


            if((_joints[5].Target > gripperVal) && (Payload == null))
            {
                _attachPayload = true;
            }
            else if ((_joints[5].Target < gripperVal) && (Payload != null))
            {
                _dropPayload = true;
            }

            // set the target values on the joint descriptors
            _joints[0].Target = baseVal;
            _joints[1].Target = shoulderVal;
            _joints[2].Target = elbowVal;
            _joints[3].Target = wristVal;
            _joints[4].Target = rotateVal;
            _joints[5].Target = gripperVal;

            // calculate a speed value for each joint that will cause it to complete its motion in the specified time
            for(int i=0; i<6; i++)
                _joints[i].Speed = Math.Abs(_joints[i].Target - _joints[i].Current) / time;

            // set this flag so that the motion is evaluated in the update method
            _moveToActive = true;

            // keep a pointer to the response port so we can post a result message to it.
            _moveToResponsePort = responsePort;
            
            return responsePort;
        }

        // These entities are used to calculate the public Origin and Direction properties so we need 
        // to keep a persistent reference to them.
        SingleShapeSegmentEntity LeftGripEntity = null;
        SingleShapeSegmentEntity RightGripEntity = null;
        SingleShapeSegmentEntity L4Entity = null;

        double _prevTime = 0;
        // This method updates the base entity and then updates the joints if a movement is active.
        public override void Update(FrameUpdate update)
        {
            // The joint member in each joint description needs to be set after all of the joints
            // have been created.  If this has not been done yet, do it now.
            if (_joints[0].Joint == null)
            {
                VisualEntity entity = this;
                if (entity.Children.Count > 0)
                {
                    entity = entity.Children[0];
                    _joints[0].Joint = (PhysicsJoint)entity.ParentJoint;
                    if (entity.Children.Count > 0)
                    {
                        entity = entity.Children[0];
                        _joints[1].Joint = (PhysicsJoint)entity.ParentJoint;
                        if (entity.Children.Count > 0)
                        {
                            entity = entity.Children[0];
                            _joints[2].Joint = (PhysicsJoint)entity.ParentJoint;
                            if (entity.Children.Count > 0)
                            {
                                entity = entity.Children[0];
                                _joints[3].Joint = (PhysicsJoint)entity.ParentJoint;
                                if (entity.Children.Count > 0)
                                {
                                    entity = entity.Children[0];
                                    _joints[4].Joint = (PhysicsJoint)entity.ParentJoint;
                                    if (entity.Children.Count > 1)
                                    {
                                        // The gripper has one virtual joint but two physical joints, 
                                        // one for each part of the gripper.
                                        _joints[5].Joint = (PhysicsJoint)entity.Children[0].ParentJoint;
                                        _joints[5].Joint2 = (PhysicsJoint)entity.Children[1].ParentJoint;
                                        LeftGripEntity = (SingleShapeSegmentEntity)entity.Children[0];
                                        RightGripEntity = (SingleShapeSegmentEntity)entity.Children[1];
                                        L4Entity = (SingleShapeSegmentEntity)entity;
                                    }
                                }
                            }
                        }
                    }
                }
            }

            base.Update(update);
            
            // update joints if necessary
            if (_moveToActive)
            {
                bool done = true;
                // Check each joint and update it if necessary.
                if (_joints[0].NeedToMove(_epsilon))
                {
                    done = false;

                    Vector3 normal = _joints[0].Joint.State.Connectors[0].JointNormal;
                    _joints[0].UpdateCurrent(_prevTime);
                    _joints[0].Joint.SetAngularDriveOrientation(
                        Quaternion.FromAxisAngle(normal.X, normal.Y, normal.Z, Conversions.DegreesToRadians(_joints[0].Current)));
                }
                if (_joints[1].NeedToMove(_epsilon))
                {
                    done = false;

                    Vector3 axis = _joints[1].Joint.State.Connectors[1].JointAxis;
                    _joints[1].UpdateCurrent(_prevTime);
                    _joints[1].Joint.SetAngularDriveOrientation(
                        Quaternion.FromAxisAngle(1, 0, 0, Conversions.DegreesToRadians(_joints[1].Current)));
                }

                if (_joints[2].NeedToMove(_epsilon))
                {
                    done = false;

                    Vector3 axis = _joints[2].Joint.State.Connectors[0].JointNormal;
                    _joints[2].UpdateCurrent(_prevTime);
                    _joints[2].Joint.SetAngularDriveOrientation(
                        Quaternion.FromAxisAngle(axis.X, axis.Y, axis.Z, Conversions.DegreesToRadians(_joints[2].Current)));
                }

                if (_joints[3].NeedToMove(_epsilon))
                {
                    done = false;

                    Vector3 axis = _joints[3].Joint.State.Connectors[0].JointNormal;
                    _joints[3].UpdateCurrent(_prevTime);
                    _joints[3].Joint.SetAngularDriveOrientation(
                        Quaternion.FromAxisAngle(axis.X, axis.Y, axis.Z, Conversions.DegreesToRadians(_joints[3].Current)));
                }

                if (_joints[4].NeedToMove(_epsilon))
                {
                    done = false;

                    Vector3 axis = _joints[4].Joint.State.Connectors[0].JointAxis;
                    _joints[4].UpdateCurrent(_prevTime);
                    _joints[4].Joint.SetAngularDriveOrientation(
                        Quaternion.FromAxisAngle(axis.X, axis.Y, axis.Z, Conversions.DegreesToRadians(_joints[4].Current)));
                }

                if (_joints[5].NeedToMove(_epsilon/100f))
                {
                    done = false;
                    _joints[5].UpdateCurrent(_prevTime);
                    float jointValue = _joints[5].Current / 2;
                    if (_joints[5].Joint != null)
                        _joints[5].Joint.SetLinearDrivePosition(new Vector3(-jointValue, 0, 0));
                    if (_joints[5].Joint2 != null)
                        _joints[5].Joint2.SetLinearDrivePosition(new Vector3(jointValue, 0, 0));
                }

                if (done)
                {
                    // no joints needed to be updated, the movement is finished
                    if (_attachPayload)
                    {
                        // we are in the process of attaching a payload object after the motion has completed
                        if (_intersect == null)
                        {
                            // haven't yet cast the intersection ray, do it now
                            _intersect = new IntersectRay(new Ray(Origin, Direction));
                            SimulationEngine.GlobalInstancePort.Post(_intersect);
                        }
                        List<TriangleIntersectionRecord> results = _intersect.ResponsePort.Test<List<TriangleIntersectionRecord>>();

                        if (results != null)
                        {
                            // we've received the intersection results, see if we need to attach a payload
                            AttachPayload(results);
                            if (_payload != null)
                            {
                                // create a joint to hold the payload
                                _payload.PhysicsEntity.UpdateState(false);
                                L4Entity.PhysicsEntity.UpdateState(false);
                                Vector3 jointLocation = TypeConversion.FromXNA(xna.Vector3.Transform(
                                    TypeConversion.ToXNA(_payload.State.Pose.Position),
                                    xna.Matrix.Invert(L4Entity.World)));

                                Vector3 normal = new Vector3(0, 1, 0);
                                Vector3 axis = new Vector3(1, 0, 0);

                                // calculate a joint orientation that will preserve the orientation relationship between L4Entity and the payload
                                Vector3 parentNormal = Quaternion.Rotate(L4Entity.State.Pose.Orientation, normal);
                                Vector3 parentAxis = Quaternion.Rotate(L4Entity.State.Pose.Orientation, axis);
                                Vector3 thisNormal = Quaternion.Rotate(_payload.State.Pose.Orientation, normal);
                                Vector3 thisAxis = Quaternion.Rotate(_payload.State.Pose.Orientation, axis);
                                EntityJointConnector[] payloadConnectors = new EntityJointConnector[2]
                                {
                                    new EntityJointConnector(_payload, thisNormal, thisAxis, new Vector3(0, -0.003f, 0)),
                                    new EntityJointConnector(L4Entity, parentNormal, parentAxis, jointLocation)
                                };
                                _payloadJoint = PhysicsJoint.Create(new JointProperties((JointAngularProperties)null, payloadConnectors));
                                _payloadJoint.State.Name = "Payload Joint";
                                PhysicsEngine.InsertJoint(_payloadJoint);
                                // the payload is now fixed to the L4Entity
                            }
                            // the payload attach is complete
                            _attachPayload = false;
                            _intersect = null;
                            // the motion is also complete, send the completion message
                            _moveToActive = false;
                            _moveToResponsePort.Post(new SuccessResult());
                        }
                    }
                    else
                    {
                        // no payload attach required, just send the completion message
                        _moveToActive = false;
                        _moveToResponsePort.Post(new SuccessResult());
                    }
                }
            }
            
            // If we detected that it is time to drop the payload, do it now
            if ((_payload != null) && _dropPayload)
            {
                PhysicsEngine.DeleteJoint(_payloadJoint);
                _payloadJoint = null;
                _payload = null;
                _dropPayload = false;
            }

            _prevTime = update.ElapsedTime;
        }

        // This property is used to generate a world-space ray originating at the center of L4Entity 
        public Vector3 Origin
        {
            get
            {
                if ((L4Entity == null) || (L4Entity.PhysicsEntity == null))
                    return new Vector3(0, 0, 0);

                L4Entity.PhysicsEntity.UpdateState(false);
                return L4Entity.State.Pose.Position;
            }
        }

        // This property is used to generate a world-space ray along the Y axis of the L4Entity
        public Vector3 Direction
        {
            get
            {
                if ((LeftGripEntity == null) || (LeftGripEntity.PhysicsEntity == null) ||
                    (RightGripEntity == null) || (RightGripEntity.PhysicsEntity == null))
                    return new Vector3(0, 1, 0);

                LeftGripEntity.PhysicsEntity.UpdateState(false);
                RightGripEntity.PhysicsEntity.UpdateState(false);
                Vector3 dir = new Vector3(
                    (RightGripEntity.State.Pose.Position.X + LeftGripEntity.State.Pose.Position.X) / 2,
                    (RightGripEntity.State.Pose.Position.Y + LeftGripEntity.State.Pose.Position.Y) / 2,
                    (RightGripEntity.State.Pose.Position.Z + LeftGripEntity.State.Pose.Position.Z) / 2);
                dir = dir - Origin;
                dir = Vector3.Normalize(dir);
                return dir;
            }
        }

        // These private members are used to keep track of the payload state
        VisualEntity _payload = null;   // reference to the current payload
        bool _dropPayload = false;  // flag indicating that payload needs to be dropped
        bool _attachPayload = false; // flag indicating payload needs to be attached
        IntersectRay _intersect = null; // keeps track of whether the intersect request has been sent
        PhysicsJoint _payloadJoint = null; // holds the joint which attaches the payload

        // provide public access to the current payload
        public VisualEntity Payload
        {
            get { return _payload; }
        }

        // once a ray has been cast into the environment, this method interprets the results and 
        // sets a payload in _payload if one is found within the grippers
        public void AttachPayload(List<TriangleIntersectionRecord> results)
        {
            foreach (TriangleIntersectionRecord candidate in results)
            {
                if (candidate.OwnerEntity.GetType() == typeof(SingleShapeSegmentEntity))
                    continue;

                if (candidate.IntersectionDistance > Grip)
                    break;

                _payload = candidate.OwnerEntity;
            }
        }
    }

    /// <summary>
    /// This camera model is for a camera meant to be attached as
    /// a child of another entity. It is free to move in yaw, pitch,
    /// and roll unlike the FirstPersonCamera. It ignores keyboard
    /// and mouse input.
    /// </summary>
    public class AttachedCameraModel : Camera
    {
        public VisualEntity Parent = null;
        public override void Update(double elapsedRealTime, bool hasFocus)
        {
            // no need to update this camera, the keyboard and mouse don't affect it
        }
        public override void SetViewParameters(Microsoft.Xna.Framework.Vector3 eyePt, Microsoft.Xna.Framework.Vector3 lookAtPt)
        {
            // if there is a parent set, the camera matrix is the inverse of the parent world matrix.
            if (Parent != null)
                _viewMatrix = xna.Matrix.Invert(Parent.World);
        }
    }

    class AttachedCameraEntity : CameraEntity
    {
        public AttachedCameraEntity()
            : base()
        {
        }

        public override void Initialize(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            base.Initialize(device, physicsEngine);

            // replace the camera model used by this camera entity using reflection
            System.Reflection.FieldInfo fInfo = GetType().BaseType.GetField("_frameworkCamera",
                System.Reflection.BindingFlags.Instance |
                System.Reflection.BindingFlags.NonPublic);
            if (fInfo != null)
            {
                AttachedCameraModel newCamera = new AttachedCameraModel();
                newCamera.Parent = this;
                fInfo.SetValue(this, newCamera);
            }

            SetProjectionParameters(
                ViewAngle,
                ((float)ViewSizeX / (float)ViewSizeY),
                Near,
                Far,
                ViewSizeX,
                ViewSizeY);
        }
    }

    [DataContract]
    public class Domino : SingleShapeEntity
    {
        public Domino()
        {
        }

        public Domino(Vector3 Position, int which)
        {
            BoxShape = new BoxShape(new BoxShapeProperties(
                0.2f,
                new Pose(),
                new Vector3(0.010f, 0.050f, 0.025f)));

            State.Name = "Domino" + which.ToString();
            State.Pose.Position = Position;
            State.Pose.Orientation = new Quaternion(0, 0, 0, 1);
            State.Assets.Mesh = "Domino.obj";

            // Dominos have relatively low friction
            MaterialProperties lowFrictionMaterial = new MaterialProperties(
                "Low Friction", 0.0f, 0.1f, 0.2f);

            BoxShape.State.Material = lowFrictionMaterial;
        }
    }



    #region WinForms communication
    public class FromWinformEvents : Port<FromWinformMsg>
    {
    }

    public class FromWinformMsg
    {
        public enum MsgEnum
        {
            Loaded,
            MoveToPosition,
            Reset,
            Test,
            Start,
            ReverseDominos,
            Park,
            ToppleDominos,
            RandomMove
        }

        private string[] _parameters;
        public string[] Parameters
        {
            get { return _parameters; }
            set { _parameters = value; }
        }

        private MsgEnum _command;
        public MsgEnum Command
        {
            get { return _command; }
            set { _command = value; }
        }

        private object _object;
        public object Object
        {
            get { return _object; }
            set { _object = value; }
        }

        public FromWinformMsg(MsgEnum command, string[] parameters)
        {
            _command = command;
            _parameters = parameters;
        }
        public FromWinformMsg(MsgEnum command, string[] parameters, object objectParam)
        {
            _command = command;
            _parameters = parameters;
            _object = objectParam;
        }
    }

    public class MoveToPositionParameters
    {
        public float X;
        public float Y;
        public float Z;
        public float GripAngle;
        public float GripRotation;
        public float Grip;
        public float Time;
    }
    #endregion


    // Utility methods combined into a single class
    public class Conversions
    {
        public static float DegreesToRadians(float degrees)
        {
            return (float)(degrees * Math.PI / 180);
        }

        public static float RadiansToDegrees(float radians)
        {
            return (float)(radians * 180 / Math.PI);
        }

        public static float InchesToMeters(float inches)
        {
            return (float)(inches * 0.0254);
        }

    }

    public static class Contract
    {
        public const string Identifier = "http://www.promrds.com/contracts/2007/10/simulatedlynxl6arm.html";
    }

    [ServicePort]
    public class SimulatedLynxL6ArmOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop>
    {
    }
}
